#include "zf_common_headfile.h"
#include "Gyroscope.h"

Gyroscope_Struct Gyroscope;

void Zero_Point_Detect(void)//读取零飘
{
    uint8 i;
    int zero_point_y_accu=0;
    int zero_point_z_accu=0;
    for(i=0;i<=100;i++)//积累100次，求取平均值，获取当前零飘
    {
        imu660ra_get_gyro();
        zero_point_y_accu+=imu660ra_gyro_y;//
        zero_point_z_accu+=imu660ra_gyro_z;
        system_delay_ms(3);//记得换成本系统的delay
    }
    Gyroscope.ZeroDrift_gyro_y=zero_point_y_accu/100.0;
    Gyroscope.ZeroDrift_gyro_z=zero_point_z_accu/100.0;
}

void Gyroscope_GetData(void)
{
    int16 gyro_z =0;
    imu660ra_get_gyro();
    gyro_z=imu660ra_gyro_z;
    Gyroscope.AngleSpeed = ((gyro_z-Gyroscope.ZeroDrift_gyro_z) * GYRO_SENS) * DT;
}

//-------------------------------------------------------------------------------------------------------------------
//  @brief      陀螺仪计算航偏角角度
//  @param
//  @return     void
//-------------------------------------------------------------------------------------------------------------------
void Get_Gyroscope_Angle(void)
{
    float K=0.7;
    Gyroscope.FJ_gyro_z = imu660ra_gyro_z;
    Gyroscope.FJ_LastAngleSpeed=Gyroscope.FJ_AngleSpeed;
    Gyroscope.FJ_AngleSpeed += ((Gyroscope.FJ_gyro_z-Gyroscope.ZeroDrift_gyro_z) * GYRO_SENS)*DT;
    Gyroscope.FJ_Angle = Gyroscope.FJ_AngleSpeed*K+Gyroscope.FJ_LastAngleSpeed*(1-K);                //向左为正
    Gyroscope.FJ_Angle = Gyroscope.FJ_Angle > Gyroscope.FJ_Angle_Max ? Gyroscope.FJ_Angle_Max : Gyroscope.FJ_Angle;
    Gyroscope.FJ_Angle = Gyroscope.FJ_Angle < Gyroscope.FJ_Angle_Min ? Gyroscope.FJ_Angle_Min : Gyroscope.FJ_Angle;
    Gyroscope.FJ_Angle = -Gyroscope.FJ_Angle;
}
//-------------------------------------------------------------------------------------------------------------------
//  @brief      陀螺仪计算俯仰角角度
//  @param
//  @return     void
//-------------------------------------------------------------------------------------------------------------------
void Get_Gyroscope_Pitch(void)
{
    float K=0.7;
    Gyroscope.FJ_LastPitchSpeed=Gyroscope.FJ_PitchSpeed;
    Gyroscope.FJ_gyro_y = imu660ra_gyro_y;
    Gyroscope.FJ_PitchSpeed += ((Gyroscope.FJ_gyro_y-Gyroscope.ZeroDrift_gyro_y) * GYRO_SENS)*DT;
    Gyroscope.FJ_Pitch = Gyroscope.FJ_PitchSpeed*K+Gyroscope.FJ_LastPitchSpeed*(1-K);

    Gyroscope.FJ_Pitch = Gyroscope.FJ_Pitch > 40 ? 40 : Gyroscope.FJ_Pitch;
    Gyroscope.FJ_Pitch = Gyroscope.FJ_Pitch < (-40) ? (-40) : Gyroscope.FJ_Pitch;
}
//-------------------------------------------------------------------------------------------------------------------
//  @brief      俯仰角角度清算
//  @param
//  @return     void
//  @note
//-------------------------------------------------------------------------------------------------------------------
void Clear_Gyroscope_Pitch(void)
{
    Gyroscope.FJ_Pitch = 0;
    Gyroscope.FJ_gyro_y = 0;
    Gyroscope.FJ_PitchSpeed = 0;
    Gyroscope.FJ_LastPitchSpeed = 0;
}
//-------------------------------------------------------------------------------------------------------------------
//  @brief      偏航角角度清算
//  @param
//  @return     void
//  @note
//-------------------------------------------------------------------------------------------------------------------
void Clear_Gyroscope_Angle(void)
{
    Gyroscope.FJ_Angle = 0;
    Gyroscope.FJ_gyro_z = 0;
    Gyroscope.FJ_AngleSpeed = 0;
    Gyroscope.FJ_LastAngleSpeed = 0;
}

#define Kp      10.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki      0.008f                       // integral gain governs rate of convergence of gyroscope biases
#define halfT   0.001f                   // half the sample period

static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
static float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error

static float yaw = 0;
static float pitch = 0;
static float roll = 0;

void IMU_Update(float gx, float gy, float gz, float ax, float ay, float az)
{
    float norm;
    float vx, vy, vz;
    float ex, ey, ez;

    float q0q0 = q0 * q0;
    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    //float q0q3 = q0 * q3;
    float q1q1 = q1 * q1;
    //float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q2 = q2 * q2;
    float q2q3 = q2 * q3;
    float q3q3 = q3 * q3;

    if (ax * ay * az == 0)
    {
        return;
    }

    norm = sqrt(ax * ax + ay * ay + az * az);       //
    ax = ax / norm;
    ay = ay / norm;
    az = az / norm;

    // estimated direction of gravity and flux (v and w)
    vx = 2 * (q1q3 - q0q2);
    vy = 2 * (q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3 ;

    // error is sum of cross product between reference direction of fields and direction measured by sensors
    ex = (ay * vz - az * vy) ;
    ey = (az * vx - ax * vz) ;
    ez = (ax * vy - ay * vx) ;

    exInt = exInt + ex * Ki;
    eyInt = eyInt + ey * Ki;
    ezInt = ezInt + ez * Ki;

    // adjusted gyroscope measurements
    gx = gx + Kp * ex + exInt;
    gy = gy + Kp * ey + eyInt;
    gz = gz + Kp * ez + ezInt;

    // integrate quaternion rate and normalise
    q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
    q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
    q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
    q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;

    // normalise quaternion
    norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 = q0 / norm;
    q1 = q1 / norm;
    q2 = q2 / norm;
    q3 = q3 / norm;

    yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 57.3; // unit:degree
    pitch  = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3; // unit:degree
    roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // unit:degree
}
